#include "reference_line_provider.h"

void ReferenceLineProvider::Provide(const RoutingPath &routing_path,
                                    const LocalizationEstimate &localzation,
                                    const ReferenceLine &pre_reference_line,
                                    ReferenceLine *reference_line)

{
}

// 1.寻找匹配点,计算匹配点在全局路径的投影.//此处与老王有所区别，将存储上一次的匹配点的功能放在Provider函数内，另外是首次运行由provide函数做判断
//公共函数，其他地方也可以调用，因此不能在其中操作其私有变量
void ReferenceLineProvider::FindMatchAndProjectPoint(const ReferenceLine &frenet_path, const std::vector<MapPoint> &map_points,
                                                     const int index_start_search, const int increase_count_limit,
                                                     std::vector<ReferencePoint> &match_points, std::vector<ReferencePoint> &project_points)
{
    auto frenet_path_points = frenet_path.reference_points();
    int size = frenet_path_points.size();
    int increase_count = 0;

    for (int i = 0; i < map_points.size(); i++)
    {
        double min_distance = DBL_MAX;
        for (int j = index_start_search; j < size; j++)
        {
            double distance = pow(map_points[j].x - frenet_path_points[i].x, 2) + pow(map_points[j].y - frenet_path_points[i].y, 2);
            if (distance < min_distance)
            {
                //如果距离小于最小距离，说明距离在递减
                min_distance = distance;
                match_points[i].index = j;
                increase_count = 0;
            }
            else
            {
                //如果距离增加，则计数+1。增加此部分为了避免在小半径连续转弯的情况，自车从前一时刻到当前时刻行走了具有两个极小值得道路，
                //而错误地将第一个极小值作为投影点，实际上是第二个才是投影点
                increase_count = increase_count + 1;
            }
            //如果distance连续增加50次就不要再遍历了，节省时间
            if (increase_count > increase_count_limit)
                break;
        }

        match_points[i].x = frenet_path_points[match_points[i].index].x;
        match_points[i].y = frenet_path_points[match_points[i].index].y;
        match_points[i].heading = frenet_path_points[match_points[i].index].x;
        match_points[i].kappa = frenet_path_points[match_points[i].index].y;

        //根据匹配点求投影点

        // 计算匹配点的方向向量与法向量
        std::pair<double, double> vector_match_point = std::make_pair(match_points[i].x, match_points[i].y);
        std::pair<double, double> vector_match_point_direction = std::make_pair(cos(match_points[i].heading), sin(match_points[i].heading));
        //声明待投影点的位矢
        std::pair<double, double> vector_r = std::make_pair(map_points[i].x, map_points[i].y);

        //通过匹配点计算投影点
        std::pair<double, double> vector_d = std::make_pair(vector_r.first - vector_match_point.first, vector_r.second - vector_match_point.second);
        double ds = vector_d.first * vector_match_point_direction.first + vector_d.second * vector_match_point_direction.second; //向量点乘

        // vector_proj_point = vector_match_point + ds * vector_match_point_direction;
        project_points[i].x = match_points[i].x + ds * vector_match_point_direction.first;
        project_points[i].x = match_points[i].y + ds * vector_match_point_direction.second;
        project_points[i].heading = match_points[i].heading + match_points[i].kappa * ds;
        project_points[i].kappa = match_points[i].kappa;
    }
}

// 2.根据自车位置的匹配点，截取参考线    首次截取调用参考线平滑器，进行参考线平滑
void ReferenceLineProvider::GetReferenceLine(const ReferenceLine &frenet_path, const int host_match_point_index, ReferenceLine *reference_line)
{
    int start_index = -1;
    int len = frenet_path.reference_points().size();
    //% 匹配点后面的点太少了，不够30个
    if (host_match_point_index - 30 < 1)
        start_index = 1;
    //% 匹配点前面的点太少了，不够150个
    else if (host_match_point_index + 150 > len)
        start_index = len - 180;
    //正常情况
    else
        start_index = host_match_point_index - 30;

    std::vector<ReferencePoint>::const_iterator it_start = frenet_path.reference_points().begin() + start_index;
    std::vector<ReferencePoint>::const_iterator it_end = frenet_path.reference_points().begin() + start_index + 180;
    std::vector<ReferencePoint> reference_points(it_start, it_end);
    reference_line->set_reference_points(reference_points);
}

// 4.非首次截取进行平滑轨迹的拼接

// 1.1 由全局路径转换参考线，即(x,y)->(x,y,heading,kappa)
/*
%该函数将计算path的切线方向与x轴的夹角和曲率
%输入：path_x,y路径坐标
%输出：path heading kappa 路径的heading和曲率
%原理 heading = arctan(dy/dx);
%     kappa = dheading/ds;
%     ds = (dx^2 + dy^2)^0.5
*/
// const对象只能访问const类型的成员
void ReferenceLineProvider::RoutingPathToFrenetPath(const RoutingPath &routing_path, ReferenceLine *frenet_path)
{
    auto points = routing_path.routing_path_points();
    int size = points.size();
    /*此部分可以通过构造2d矢量类，来统一计算，见apollo common::math::Vec2D
    此处使用的是vector编程，后面再考虑使用eigen怎么操作
    */
    //前向差分，后向差分，差分中值
    //求heading
    std::vector<MapPoint> points_diff, points_diff_pre, points_diff_after, points_diff_fianl;
    std::vector<double> ds_final, points_heading;
    for (int i = 0; i < size - 1; i++)
    {
        points_diff[i].x = points[i + 1].x - points[i].x;
        points_diff[i].y = points[i + 1].y - points[i].y;
    }

    points_diff_pre = points_diff;
    points_diff_after = points_diff;
    points_diff_pre.insert(points_diff_pre.begin(), points_diff.front());
    points_diff_after.emplace_back(points_diff.back());
    for (int i = 0; i < size; i++)
    {
        points_diff_fianl[i].x = (points_diff_pre[i].x + points_diff_after[i].x) / 2;
        points_diff_fianl[i].y = (points_diff_pre[i].y + points_diff_after[i].y) / 2;
        ds_final[i] = sqrt(pow(points_diff_fianl[i].x, 2) + pow(points_diff_fianl[i].y, 2));
        points_heading[i] = atan2(points_diff_fianl[i].y, points_diff_fianl[i].x);
    }

    //求kappa
    std::vector<double> heading_diff, heading_diff_pre, heading_diff_after, heading_diff_final, points_kappa;
    for (int i = 0; i < size - 1; i++)
    {
        heading_diff[i] = points_heading[i + 1] - points_heading[i];
    }
    heading_diff_pre = heading_diff;
    heading_diff_after = heading_diff;
    heading_diff_pre.insert(heading_diff_pre.begin(), heading_diff.front());
    heading_diff_after.emplace_back(heading_diff.back());

    for (int i = 0; i < size; i++)
    {
        heading_diff_final[i] = (heading_diff_pre[i] + heading_diff_after[i]) / 2;
        //为了防止dheading出现多一个2pi的错误，假设真实的dheading较小，用sin(dheading) 近似dheading
        points_kappa[i] = sin(heading_diff_final[i]) / ds_final[i];
    }
    std::vector<ReferencePoint> reference_points;
    for (int i = 0; i < size; i++)
    {
        reference_points[i].x = points[i].x;
        reference_points[i].y = points[i].y;
        reference_points[i].heading = points_heading[i];
        reference_points[i].heading = points_kappa[i];
    }
    frenet_path->set_reference_points(reference_points);
}
